/*
 * @Author: LVGRAPE
 * @Date: 2022-07-27 13:59:58
 * @LastEditTime: 2023-11-08 17:52:48
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2023-05-10 14:04:06
 * @LastEditors: EltonLi lijiaming@uavi-tech.com
 * @Description:
 * @FilePath: \ZINO_MCU_SOURCE\ZINO_SRC\ZINO_FC\sensors\acc_gyro\acc_gyro.c
 * 可以输入预定的版权声明、个性签名、空行等
 */
#include "acc_gyro.h"
#include "m_qmi8610.h"
#include "DataSave.h"
#include "fc_error.h"
#include "USART_Configuration.h"

#define ACC_CALIBRATE_CYCLESS 300
#define GYRO_CALIBRATE_CYCLESS 300

static bool qmi8610_init(acc_gyro_dev_t* dev)
{
    QMI8610_Hardware_Reset();
    // if (QMI8610_Check_ID())
    // {
    //     QMI8610_Init();
    //     dev->online = 1;
    //     if (zino_data_save.ZINO.ACC_Calibration.status)
    //     {
    //         dev->accZero.axis[0] = zino_data_save.ZINO.ACC_Calibration.accZero[0];
    //         dev->accZero.axis[1] = zino_data_save.ZINO.ACC_Calibration.accZero[1];
    //         dev->accZero.axis[2] = zino_data_save.ZINO.ACC_Calibration.accZero[2];
    //     }
    //     rt_kprintf("qmi8610 online\n");
    //     FC_ERROR_RESET(FC_ACC_GYRO_HARDFAULT);
    // }
    // else
    // {
    //     FC_ERROR_SET(FC_ACC_GYRO_HARDFAULT);
    //     rt_kprintf("QMI_Check_ID ERROR!\n");
    //     dev->online = 0;
    //     return 1;
    // }
    return 0;
}
static bool qmi8610_init_noreset(acc_gyro_dev_t* dev)
{
    // QMI8610_Hardware_Reset();
    if(!isQmi8610_ready_form_reset()) return 1;

    if (QMI8610_Check_ID())
    {
        QMI8610_Init();
        dev->online = 1;
        if (zino_data_save.ZINO.ACC_Calibration.status)
        {
            dev->accZero.axis[0] = zino_data_save.ZINO.ACC_Calibration.accZero[0];
            dev->accZero.axis[1] = zino_data_save.ZINO.ACC_Calibration.accZero[1];
            dev->accZero.axis[2] = zino_data_save.ZINO.ACC_Calibration.accZero[2];
        }
        rt_kprintf("qmi8610 online\n");
        dev->ops->acc_start_calibration(dev);
        dev->ops->gyro_start_calibration(dev);
        FC_ERROR_RESET(FC_ACC_GYRO_HARDFAULT);
    }
    else
    {
        FC_ERROR_SET(FC_ACC_GYRO_HARDFAULT);
        QMI8610_Hardware_Reset();
        // rt_kprintf("QMI_Check_ID ERROR!\n");
        dev->online = 0;
        return 1;
    }
    return 0;
}
uint8_t qmi8650_spi_read(uint8_t addr, uint8_t* readbuffer, uint16_t numToRead)
{

}
uint8_t qmi8650_i2c_read(uint8_t addr, uint8_t* readbuffer, uint16_t numToRead)
{

}
static void m_acc_start_calibration(acc_gyro_dev_t* dev)
{
    dev->accCalibrationSampleCount = ACC_CALIBRATE_CYCLESS;
}
static void m_gyro_start_calibration(acc_gyro_dev_t* dev)
{
    dev->gyroCalibrationSampleCount = ACC_CALIBRATE_CYCLESS;
}
static bool m_isAccCalibrated(acc_gyro_dev_t* dev)
{
    return (dev->accCalibrationSampleCount == 0);
}
static bool m_isGyroCalibrated(acc_gyro_dev_t* dev)
{
    return (dev->gyroCalibrationSampleCount == 0);
}
static void m_performAccCalibration(acc_gyro_dev_t* dev)
{
    for (int axis = 0;axis < 3;axis++)
    {
        if (dev->accCalibrationSampleCount == ACC_CALIBRATE_CYCLESS)//first cycle
        {
            dev->accCaliSum.axis[axis] = 0.f;
        }
        dev->accCaliSum.axis[axis] += dev->acc_raw.axis[axis];
        if (dev->accCalibrationSampleCount == 1)
        {
            dev->accZero.axis[axis] = dev->accCaliSum.axis[axis] / (float)ACC_CALIBRATE_CYCLESS;
            if (axis == Z)
            {
                dev->accZero.axis[axis] -= 9.8F;
            }
            zino_data_save.ZINO.ACC_Calibration.accZero[axis] = dev->accZero.axis[axis];
            rt_kprintf("***ACC_%d:", axis);
            usartPrintVariable(" ", dev->accZero.axis[axis], 1);

        }
    }
    dev->accCalibrationSampleCount--;
    if (dev->accCalibrationSampleCount == 0)
    {
        zino_data_save.ZINO.ACC_Calibration.status = 1;
        ZINO_DATA_SAVE_ALL();
    }
}

static void m_performGyroCalibration(acc_gyro_dev_t* dev)
{
    for (int axis = 0;axis < 3;axis++)
    {
        if (dev->gyroCalibrationSampleCount == GYRO_CALIBRATE_CYCLESS)//first cycle
        {
            dev->gyroCaliSum.axis[axis] = 0;
        }
        dev->gyroCaliSum.axis[axis] += dev->gyro_raw.axis[axis];
        if (dev->gyroCalibrationSampleCount == 1)
        {
            dev->gyroZero.axis[axis] = dev->gyroCaliSum.axis[axis] / (float)GYRO_CALIBRATE_CYCLESS;
            rt_kprintf("***GYRO_%d:", axis);
            usartPrintVariable(" ", dev->gyroZero.axis[axis], 1);
        }

    }
    dev->gyroCalibrationSampleCount--;
}
static bool qmi8610_getTemperature(acc_gyro_dev_t* dev)
{
    dev->temperature = QMI8610_get_raw_temp();
}
static bool qmi8610_update_acc_gyro(acc_gyro_dev_t* dev, float acc[3], gyro[3]);
{
    // static float acc[3];
    // static float gyro[3];
    if(FC_ERROR_CHECK(FC_ACC_GYRO_HARDFAULT))
    {
        dev->ops->acc_gyro_init_withoutReset(dev);
        rt_kprintf("acc_gyro_reinit...\n");
    }
    // qmi8610_get_acc_gyro_f32(acc, gyro);
    for (uint8_t axis = 0;axis < 3;axis++)
    {
        dev->acc_raw.axis[axis] = acc[axis];
        dev->gyro_raw.axis[axis] = gyro[axis];
    }
    if (FC_ERROR_CHECK(FC_ACC_CALIBRATING))
    {
        if (!dev->ops->isAccCalibrated(dev))
        {
            m_performAccCalibration(dev);
        }
        if (!dev->ops->isGyroCalibrated(dev))
        {
            m_performGyroCalibration(dev);
        }
    }
    for (int i = 0;i < 3;i++)
    {
        /**校准值 */
        dev->accCalibrated.axis[i] = dev->acc_raw.axis[i] - dev->accZero.axis[i];
        dev->gyroCalibrated.axis[i] = dev->gyro_raw.axis[i] - dev->gyroZero.axis[i];

        dev->accLpf.axis[i] = dev->accCalibrated.axis[i];
        dev->gyroLpf.axis[i] = dev->gyroCalibrated.axis[i];
    }
    return 0;
}
struct acc_gyro_ops qmi8610_ops = {
    .hardware_reset = QMI8610_Hardware_Reset,
    .acc_gyro_init = qmi8610_init,
    .acc_gyro_init_withoutReset = qmi8610_init_noreset,
    .acc_gyro_update = qmi8610_update_acc_gyro,
    .acc_start_calibration = m_acc_start_calibration,
    .gyro_start_calibration = m_gyro_start_calibration,
    .isAccCalibrated = m_isAccCalibrated,
    .isGyroCalibrated = m_isGyroCalibrated,
    .getTemperature = qmi8610_getTemperature,
    .read = qmi8650_i2c_read,
};
acc_gyro_dev_t acc_gyro_dev_qmi8610 =
{
    .ops = &qmi8610_ops,
    .online = 0,
};
